
#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <queue>

struct map_grid{
    double distance;
    int x;
    int y;
};

class CustomMap{
public:
    CustomMap(ros::NodeHandle& n,std::string& topic_name)
    {
        nh = n;
        std::cout<<topic_name<<std::endl;
        bool get_map = false;
        while(!get_map)
        {
            auto msg = ros::topic::waitForMessage<nav_msgs::OccupancyGrid>(topic_name, ros::Duration(5));
            if(msg)
            {
                init_map(msg);
                get_map = true;
            }
            else
                ROS_ERROR("Wait for map timeout");
        }
    }   

    double get_distance(double x,double y)
    {
        int map_x = (x-origin_x_)/resolution_;
        int map_y = (y-origin_y_)/resolution_;
        if(map_x<0 || map_x>=width_ || map_y<0 || map_y >= height_)
        {
            return -1;
        }
        double distance = custom_map[map_x][map_y].distance;
        return distance;
    }

    double get_nearest_obstacle(double x,double y,double& obstacle_x,double& obstacle_y)
    {
        int map_x = (x-origin_x_)/resolution_;
        int map_y = (y-origin_y_)/resolution_;
        if(map_x<0 || map_x>=width_ || map_y<0 || map_y >= height_)
        {
            obstacle_x = std::max(std::min(map_x,width_),0)*resolution_+origin_x_;
            obstacle_y = std::max(std::min(map_y,height_),0)*resolution_+origin_y_;
            return sqrt(pow(x-obstacle_x,2)+pow(y-obstacle_y,2));
        }
        double distance = custom_map[map_x][map_y].distance*resolution_;
        obstacle_x = custom_map[map_x][map_y].x*resolution_+origin_x_;
        obstacle_y = custom_map[map_x][map_y].y*resolution_+origin_y_;
        return distance;
    }



private:
    void init_map(const nav_msgs::OccupancyGridConstPtr& msg)
    {
        width_ = msg->info.width;
        height_ = msg->info.height;
        origin_x_ = msg->info.origin.position.x;
        origin_y_ = msg->info.origin.position.y;
        resolution_ = msg->info.resolution;
        
        custom_map = std::vector<std::vector<map_grid>>(width_,std::vector<map_grid>(height_,map_grid()));
        auto visited = std::vector<std::vector<bool>>(width_,std::vector<bool>(height_,false));
        int occ = 0;
        int free = 0;
        std::queue<int> map_queue;
        for(int i=0;i<msg->data.size();i++)
        {
            
            if(msg->data[i] == 100)
            {
                int x =  i%width_;
                int y = i/width_;
                custom_map[x][y].distance = 0;
                custom_map[x][y].x = x;
                custom_map[x][y].y = y;
                map_queue.push(i);
                visited[x][y] = true;
            }
        }

        while(map_queue.size() != 0)
        {
            auto front = map_queue.front();
            map_queue.pop();
            int x = front%width_;
            int y = front/width_;
            if(x+1<width_ && !visited[x+1][y])
            {
                dfs(custom_map[x][y],x+1,y,visited);
                map_queue.push((x+1)+y*width_);
            }
            if(x-1>=0 && !visited[x-1][y])
            {
                dfs(custom_map[x][y],x-1,y,visited);
                map_queue.push((x-1)+y*width_);
            }
            if(y-1>=0&&!visited[x][y-1])
            {
                dfs(custom_map[x][y],x,y-1,visited);
                map_queue.push(x+(y-1)*width_);
            }
            if(y+1<height_&&!visited[x][y+1])
            {
                dfs(custom_map[x][y],x,y+1,visited);
                map_queue.push(x+(y+1)*width_);
            }
        }
        double max_dis = 0;
        int max_x,max_y,occ_x,occ_y;
        for(int i=0;i<custom_map.size();i++)
        {
            for(int j=0;j<custom_map[i].size();j++)
            {
                if(custom_map[i][j].distance>max_dis)
                {
                    max_x = i;
                    max_y = j;
                    occ_x = custom_map[i][j].x;
                    occ_y = custom_map[i][j].y;
                    max_dis = custom_map[i][j].distance;
                }
            }
        }
        std::cout<<max_dis<<std::endl;
    }

    void dfs(const map_grid& parent_grid,int x,int y,std::vector<std::vector<bool>>& visited)
    {
        custom_map[x][y].x = parent_grid.x;
        custom_map[x][y].y = parent_grid.y;
        custom_map[x][y].distance = sqrt(pow(x-custom_map[x][y].x,2)+pow(y-custom_map[x][y].y,2));
        
        visited[x][y] = true;
    }



private:
    ros::NodeHandle nh;
    ros::Subscriber map_sub;
    std::vector<std::vector<map_grid>> custom_map;

    int width_;
    int height_;
    double resolution_;
    double origin_x_;
    double origin_y_;
};